/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of Unit interface Server.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "UnitServer.hpp"
#include "UnitData.hpp"
#include <algorithm>

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Unit;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CUnitServer::CUnitServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                           const int aInterfaceMinor,
                           const std::string &aDescription,
                           const std::string &aInstanceName)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_UNIT, aInterfaceMinor, aInstanceName),
    CSync(1,1),
    CThread(1),
    iServiceDescription(aDescription),
    iUnitSequenceNumber(0),
    iUnitServerCallback(NULL),
    iAvailableUnitMap(),
    iAvailableUnitMapSeq(0),
    iActiveUnitMap(),
    iActiveUnitMapSeq(0),
    iActiveUnitUserPtrMap(),
    iUnitCommandQueue()
{
}
//*****************************************************************************

CUnitServer::~CUnitServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CUnitServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!IsActive()) {
    // Register service.
    RegisterProvidedService("MaCI-UnitServer; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-UnitServer-Cmd; '" + iServiceDescription + "'");
    
    // Start the handler Thread
    SetActive(true);
    CThread::RunThread();
    
    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed UnitServer instance %p.", this);
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CUnitServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (IsActive()) {
    // Stop!
    SetActive(false);

    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();

    // Stop the handler Thread
    CThread::WaitThread();    
    
    dPrint(4,"Succesfully Close():ed UnitServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

uint32_t CUnitServer::RegisterAvailableUnit(const EUnitType &aUnitType,
                                            const std::string &aImplementation)
{
  Lock();
  uint32_t result = 0;
  const uint32_t available_id = ++iAvailableUnitMapSeq;
  const TAvailableUnit newunit = TAvailableUnit(available_id, 
                                                aUnitType, 
                                                aImplementation);
  if (std::find(iAvailableUnitMap.begin(), iAvailableUnitMap.end(), newunit) == iAvailableUnitMap.end()) {
    iAvailableUnitMap.insert(std::make_pair(available_id, newunit));
    result = available_id;
  }
  

  Unlock();

  return result;
}
//*****************************************************************************

uint32_t CUnitServer::RegisterActiveUnit(const uint32_t &aAvailableId,
                                         const std::string &aInstanceName,
                                         void *aUserPtr)
{
  Lock();

  uint32_t result = 0;
  const uint32_t active_id = ++iActiveUnitMapSeq;
  const TActiveUnit newunit = TActiveUnit(aAvailableId, 
                                          active_id, 
                                          aInstanceName);
  if (std::find(iActiveUnitMap.begin(), iActiveUnitMap.end(), newunit) == iActiveUnitMap.end()) {
    iActiveUnitMap.insert(std::make_pair(active_id, newunit));
    iActiveUnitUserPtrMap[active_id] = aUserPtr;
    result = active_id;

    dPrint(ODTEST,"Registered Active unit");
  
  } else {
    dPrint(ODTEST,"Found a match! newunit: %s, existing: %s, new_id: %d, ex_id: %d",
           newunit.instance, std::find(iActiveUnitMap.begin(), iActiveUnitMap.end(), newunit)->second.instance,
           newunit.active_id, std::find(iActiveUnitMap.begin(), iActiveUnitMap.end(), newunit)->second.active_id);
    dPrint(ODTEST,"Refusing to Re-Register unit!");
  
  }
  
  Unlock();

  return result; 
}
//*****************************************************************************

bool CUnitServer::UnregisterActiveUnit(const uint32_t &aActiveId)
{
  bool result = false;
  TActiveUnitMap::iterator i = iActiveUnitMap.find(aActiveId);
  if (i != iActiveUnitMap.end()) {
    iActiveUnitMap.erase(i);

    TActiveUnitUserPtrPairMap::iterator i = iActiveUnitUserPtrMap.find(aActiveId);
    assert(i != iActiveUnitUserPtrMap.end());
    iActiveUnitUserPtrMap.erase(i);

    result = true;
  }
  
  return result;
}
//*****************************************************************************

bool CUnitServer::UpdateActiveUnitUserPtr(const uint32_t &aActiveId, void *aUserPtr)
{
  bool result = false;
  
  TActiveUnitUserPtrPairMap::iterator i = iActiveUnitUserPtrMap.find(aActiveId);

  if (i != iActiveUnitUserPtrMap.end()) {
    dPrint(ODTEST,"Updated active_id: %u 'UserPtr' from %p to %p",
           aActiveId, i->second, aUserPtr);
    i->second = aUserPtr;
    result = true;
  }

  return result;
}
//*****************************************************************************

const TAvailableUnit *CUnitServer::GetAvailableUnitByAvailableId(const uint32_t &aAvailableId)
{
  const TAvailableUnit *result = NULL;

  Lock();

  TAvailableUnitMap::const_iterator i = iAvailableUnitMap.find(aAvailableId);
  if (i != iAvailableUnitMap.end()) {
    result = &i->second;
  }
  
  Unlock();


  return result;
}
//*****************************************************************************

TActiveUnitUserPtrPair CUnitServer::GetActiveUnitByActiveId(const uint32_t &aActiveId)
{
  const TActiveUnit *au = NULL;
  void *userptr = NULL;

  Lock();

  TActiveUnitMap::const_iterator i = iActiveUnitMap.find(aActiveId);
  if (i != iActiveUnitMap.end()) {
    au = &i->second;
    TActiveUnitUserPtrPairMap::const_iterator j = iActiveUnitUserPtrMap.find(aActiveId);
    assert(j != iActiveUnitUserPtrMap.end());
    userptr = j->second;
  }
  
  Unlock();
  
  return std::make_pair(au, userptr);
}
//*****************************************************************************

MaCI::EMaCIError CUnitServer::SendReply(const CUnitData &aData, 
                                        const SMessageSource &aSource)
{
  
  EMaCIError result = KMaCIError;
  
  // Check that server is active.
  if (IsActive()) {
    
    // Send
    result = SendSynchronousMessageReply(aData.GetBinBagContainerPtr()->GetBuffer(),
                                         aData.GetBinBagContainerPtr()->GetBufferSize(),
                                         aSource);
    
  } else {
    dPrint(ODWARN, "Unable to send UnitData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;
    
  }

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CUnitServer::WaitCommand(CUnitData &aData, 
                                          SMessageSource &aSource,
                                          const unsigned int aTimeout_ms)
{
  
  MaCI::EMaCIError result = KMaCIError;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return MaCI::KMaCINotActive;
  
  Lock();
  int time_left = aTimeout_ms;
  unsigned int time_begin = ownTime_get_ms();
  // Wait for new data to arrive

  while(IsActive() && iUnitCommandQueue.empty() && time_left > 0) {
    Wait(time_left);
    time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
  }
  
  if (iUnitCommandQueue.size()) {
    dPrint(ODTEST,"%u commands in Queue", iUnitCommandQueue.size());
    CUnitData &ud = iUnitCommandQueue.front().first;
    SMessageSource &ms = iUnitCommandQueue.front().second;

    aData = ud;
    aSource = ms;

    iUnitCommandQueue.pop_front();
    result = MaCI::KMaCIOK;
  }
  Unlock();

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CUnitServer::ProcessMessage(const CUnitData &aMessage, const SMessageSource &aSource)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CUnitData reply;
  reply.CreateInternalBinBag();

  const TCommand *cmd = aMessage.GetCommand();
  if (cmd) {
    switch(cmd->cmd) {
      
    case KCommandGetAvailableUnits: {
      // Construct reply
      reply.SetTimestamp(Common::TTimestamp());
      reply.SetCommand(TCommand(KCommandGetAvailableUnits));
    
      for(EACH_IN_i(iAvailableUnitMap)) {
        bool ar = reply.AddAvailableUnit(i->second);
        assert(ar == true);
      }
      
      // Send as reply. Can be picked from queue as is - so no need
      // for extra identification information.
      SendReply(reply, aSource);
      break;
    }


      // Fallthrough commands.
    case KCommandCreateUnit: 
    case KCommandDeleteUnit: {
      // Lock, Push, Notify, Unlock
      Lock();
      iUnitCommandQueue.push_back(std::make_pair(aMessage, aSource));
      Notify();
      Unlock();
      break;
    }

  
    default:
      dPrint(2,"Unsupported UnitServer command %d received. Ignoring.",
             cmd->cmd);
      break;
    }
  }

  return result;
}
//*****************************************************************************

int CUnitServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  const unsigned int hard_error_count_limit = 10;
  unsigned int hard_error_count = 0;
  EMaCIError e;
  CUnitData dd;
  SMessageSource ms;

  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(8, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    dd.Reset();
    
    // Call the ReceiveMessage wrapper with 1s timeout. This template
    // function model is able to convert to correct type
    // automatically.
    
    e = ReceiveInterfaceMessage(dd, ms, 2000);
    if (e == KMaCIOK) {
    
      ProcessMessage(dd, ms);
      
      // Reset error counter.
      if (hard_error_count) hard_error_count = 0;
      
    } else if (e != KMaCITimeout) {
      // Increment hard error count
      ++hard_error_count;
      
      // Check whether the hard error count limit is still under limit. 
      if (hard_error_count < hard_error_count_limit) {
        // Other error, try to recover by sleeping one second.
        dPrint(10,"ProcessMessage returned error %u, trying to recover by sleeping 1000ms (%u attempts left)",
               e, hard_error_count_limit-hard_error_count);
        ownSleep_ms(1000);

      } else {
        dPrint(3,"Error Recovery failed, UnitServer ThreadFunction terminating...");

        // Break the while().
        break;
      }
      
    }
  }

  dPrint(8, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

